package com.hitqz.robot.arm.util;

import cn.hutool.json.JSONUtil;
import com.alibaba.fastjson.JSONObject;
import com.hitqz.robot.arm.constant.RokaeConstant;
import com.hitqz.robot.arm.dto.*;
import com.hitqz.robot.arm.type.OperateModeEnums;
import com.hitqz.robot.arm.type.OperateStateEnums;
import com.hitqz.robot.arm.type.PowerStateEnums;
import com.rokae.sdk.engine.RobotResult;
import com.rokae.sdk.enums.RobotT;
import com.rokae.sdk.model.commands.MoveAbsJCommand;
import com.rokae.sdk.model.commands.MoveCCommand;
import com.rokae.sdk.model.commands.MoveLCommand;
import com.rokae.sdk.model.position.CartesianPosition;
import com.rokae.sdk.model.position.JointPosition;
import com.rokae.sdk.model.robot.XMateRobot;
import lombok.extern.slf4j.Slf4j;
import org.springframework.util.StringUtils;

import java.util.*;
import java.util.concurrent.TimeUnit;

/**
 * @author kehanjiang
 * @date 2024/09/11
 */
@Slf4j
public class RokaeToolkit {
    //构造单例
    public static RokaeToolkit singleton = null;
    private RokaeToolkit() {}
    public synchronized static RokaeToolkit getInstance() {
        if (singleton == null) {
            singleton = new RokaeToolkit();
        }
        return singleton;
    }


    private final Map<String, XMateRobot> robotMap = new HashMap<>();

    /**
     * 连接机械臂，机械臂重启或者网络断开重连以后需要调用
     *
     * @param ip
     * @return
     */
    public boolean connect(String ip) {
        XMateRobot robot = robotMap.get(ip);
        if (robot != null) {
            return Boolean.TRUE;
        } else {
            robot = new XMateRobot(ip);
            robot.connect();
            robot.setLog(false);
            robotMap.put(ip, robot);
        }
        return Boolean.TRUE;
    }

    /**
     * 断开机械臂链接
     */
    public boolean disconnect(String ip) {
        if (robotMap.containsKey(ip)) {
            XMateRobot robot = robotMap.get(ip);
            robot.disconnect();
            robotMap.remove(ip);
        }
        return true;
    }

    /**
     * 获取已连接机械臂IP列表
     */
    public Set<String> getOnlineArmList() {
        return robotMap.keySet();
    }


    private boolean checkResult(RobotResult result) {
        int code = result.getCode();
        if (code != 0) {
            throw new RuntimeException(
                    String.format("操作失败！%s", JSONUtil.toJsonStr(result))
            );
        }
        return true;
    }

    /**
     * 机械臂上电
     *
     * @return
     */
    public boolean powerOn(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.setPowerState(true);
        return this.checkResult(result);
    }


    /**
     * 机械臂下电
     *
     * @return
     */
    public boolean powerOff(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.setPowerState(false);
        return this.checkResult(result);
    }

    /**
     * 机器人模式切换 0 -手动 1-自动
     */
    public boolean setOperateMode(String ip, Integer type) {//0 -手动 1-自动
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = null;
        if (type == 0) {//0 -手动
            result = robot.setOperateMode(RobotT.RobotMode.Operate_manual);
        } else if (type == 1) {//1-自动
            result = robot.setOperateMode(RobotT.RobotMode.Operate_automatic);
        }
        return this.checkResult(result);
    }

    /**
     * 获取当前设置下工具末端的位姿
     *
     * @return
     */
    public CartesianPoseDto getTcpPosition(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.getFlangePos();
        this.checkResult(result);
        List<Double> list = JSONObject.parseArray(String.valueOf(result.getData()), Double.class);
        CartesianPoseDto dto = new CartesianPoseDto();
        dto.setX(list.get(0));
        dto.setY(list.get(1));
        dto.setZ(list.get(2));
        dto.setRx(list.get(3));
        dto.setRy(list.get(4));
        dto.setRz(list.get(5));
        return dto;
    }

    /**
     * 获取当前机械臂关节角度
     *
     * @return
     */
    public JointValueDto getJointPosition(String ip) {
        XMateRobot robot = robotMap.get(ip);
        if (robot == null) {
            return new JointValueDto();
        }
        RobotResult result = robot.getJointPos();
        this.checkResult(result);
        List<Double> list = JSONObject.parseArray(String.valueOf(result.getData()), Double.class);
        JointValueDto dto = new JointValueDto();
        double[] joints = new double[]{
                list.get(0), list.get(1), list.get(2),
                list.get(3), list.get(4), list.get(5)
        };
        dto.setJointPos(joints);
        return dto;
    }

    /**
     * 关节运动
     *
     * @param dto
     * @return
     */
    public boolean jointMove(String ip, JointMoveDto dto) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        robot.moveReset();//运动重置
        MoveAbsJCommand command = new MoveAbsJCommand();
        JointPosition jointPosition = new JointPosition(dto.getJointPos());
        command.setTarget(jointPosition);
        command.setSpeed(dto.getSpeed());
        command.setZone(dto.getZone());
        RobotResult result = robot.executeCommands(List.of(command));
        return this.checkResult(result);
    }

    /**
     * 末端直线运动
     *
     * @param dto
     * @return
     */
    public boolean linearMove(String ip, LinearMoveDto dto) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        robot.moveReset();//运动重置
        double[] cartesian = new double[]{
                dto.getX(), dto.getY(), dto.getZ(),
                dto.getRx(), dto.getRy(), dto.getRz()
        };
        CartesianPosition target = new CartesianPosition(cartesian);
        MoveLCommand command = new MoveLCommand();
        command.setTarget(target);
        command.setSpeed(dto.getSpeed());
        command.setZone(dto.getZone());
        RobotResult result = robot.executeCommands(List.of(command));
        return this.checkResult(result);
    }

    /**
     * 末端圆弧运动
     *
     * @param dto
     * @return
     */
    public boolean circularMove(String ip, CircularMoveDto dto) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        robot.moveReset();//运动重置
        CartesianPoseDto endPos = dto.getEndPos();
        double[] targetCartesian = new double[]{
                endPos.getX(), endPos.getY(), endPos.getZ(),
                endPos.getRx(), endPos.getRy(), endPos.getRz()
        };
        CartesianPosition target = new CartesianPosition(targetCartesian);
        CartesianPoseDto midPos = dto.getMidPos();
        double[] auxCartesian = new double[]{
                midPos.getX(), midPos.getY(), midPos.getZ(),
                midPos.getRx(), midPos.getRy(), midPos.getRz()
        };
        CartesianPosition aux = new CartesianPosition(auxCartesian);
        MoveCCommand command = new MoveCCommand();
        command.setTarget(target);
        command.setAux(aux);
        command.setSpeed(dto.getSpeed());
        command.setZone(dto.getZone());
        RobotResult result = robot.executeCommands(List.of(command));
        return this.checkResult(result);
    }

    /**
     * 控制机器人手动模式下运动
     * <p>
     * 注意：“手动”模式下 示教器需要切换到 无示教器模式 才能上电成功
     *
     * @param ip
     * @param dto
     * @return
     */
    public boolean jog(String ip, JogMoveDto dto) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        robot.setOperateMode(RobotT.RobotMode.Operate_manual);//切到手动模式
        robot.setPowerState(true);  // 机器人上电
        RobotResult result = robot.startJog(RobotT.JogMotionSpace.jointSpace,
                dto.getRate(), dto.getStep(), dto.getNum(), dto.isDirection());
        return this.checkResult(result);
    }

    /**
     * 终止当前机械臂运动
     *
     * @return
     */
    public boolean motionAbort(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.moveStop();
        return this.checkResult(result);
    }

    /**
     * 获取机械臂状态
     *
     * @return
     */
    public RobotStatusDto getRobotStatus(String ip) {
        RobotStatusDto robotStatusDto = new RobotStatusDto();
        robotStatusDto.setPowerState(this.getRobotPowerState(ip).getCode());
        robotStatusDto.setOperateMode(this.getRobotOperateMode(ip).getCode());
        robotStatusDto.setOperateState(this.getRobotOperateState(ip).getCode());
        return robotStatusDto;
    }

    /**
     * 获取机器人上电状态
     *
     * @param ip
     * @return
     */
    public PowerStateEnums getRobotPowerState(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.getPowerState();//查询上电状态
        int powerState = -1;
        if (result.getCode() == 0) {
            String data = result.getData();
            if (StringUtils.hasText(data)) {
                JSONObject obj = JSONObject.parseObject(data);
                powerState = obj.getInteger("state");
            }
        }
        return PowerStateEnums.getByCode(powerState);
    }

    /**
     * 获取机器人操作模式
     *
     * @param ip
     * @return
     */
    public OperateModeEnums getRobotOperateMode(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result1 = robot.getOperateMode();// 查询当前操作模式
        int operateMode = -1;
        if (result1.getCode() == 0) {
            String data = result1.getData();
            if (StringUtils.hasText(data)) {
                JSONObject obj = JSONObject.parseObject(data);
                operateMode = obj.getInteger("mode");
            }
        }
        return OperateModeEnums.getByCode(operateMode);
    }

    /**
     * 获取机器人运行状态
     *
     * @param ip
     * @return
     */
    public OperateStateEnums getRobotOperateState(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result2 = robot.getOperateState(); //查询机器人运行状态
        int operateState = -1;
        if (result2.getCode() == 0) {
            String data = result2.getData();
            if (StringUtils.hasText(data)) {
                JSONObject obj = JSONObject.parseObject(data);
                operateState = obj.getInteger("operate");
            }
        }
        return OperateStateEnums.getByCode(operateState);
    }

    /**
     * 机械臂急停恢复
     *
     * @return
     */
    public boolean recoverState(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.recoverState(1);
        return this.checkResult(result);
    }

    /**
     * 机器人开启拖拽模式。
     *
     * @return
     */
    public boolean enableDrag(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        robot.setOperateMode(RobotT.RobotMode.Operate_manual);
        robot.setPowerState(false);
        try {
            TimeUnit.SECONDS.sleep(1);//等待切换模式
        } catch (Exception e) {
        }
        RobotResult result = robot.enableDrag(
                RobotT.DragParameterSpace.cartesianSpace,
                RobotT.DragParameterType.freely
        );
        return this.checkResult(result);
    }

    /**
     * 机器人停止拖拽模式。
     *
     * @return
     */
    public boolean disableDrag(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.disableDrag();
        return this.checkResult(result);
    }

    /**
     * 开始录制路径
     *
     * @return
     */
    public boolean startRecordPath(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.startRecordPath(RokaeConstant.DEFAULT_DURATION);
        return this.checkResult(result);
    }

    /**
     * 停止录制路径
     *
     * @return
     */
    public boolean stopRecordPath(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.stopRecordPath();
        return this.checkResult(result);
    }

    /**
     * 取消录制路径
     *
     * @return
     */
    public boolean cancelRecordPath(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.cancelRecordPath();
        return this.checkResult(result);
    }

    /**
     * 保存录制好的路径
     *
     * @param ip
     * @param path 路径名称
     * @return
     */
    public boolean saveRecordPath(String ip, String path) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.saveRecordPath(path);
        return this.checkResult(result);
    }

    /**
     * 保存录制好的路径
     *
     * @param ip
     * @param path - 路径名称
     * @param saveAsName - 如果已录制好一条路径但没有保存，则用该名字保存路径。如果没有未保存的路径，则将
     *             已保存的名为"name"的路径重命名为"saveAsName"
     * @return
     */
    public boolean saveRecordPath(String ip, String path, String saveAsName) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.saveRecordPath(path, saveAsName);
        return this.checkResult(result);
    }

    /**
     * 运动指令-路径回放
     *
     * @param ip
     * @param path 要回放的路径名称
     * @param rate 回放速率, 应小于 3.0, 1 为路径原始速率。注意当速率大于 1 时，可能产生驱动器无法跟随
     *             错误
     * @return
     */
    public boolean replayPath(String ip, String path, double rate) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.replayPath(path, rate);
        robot.moveStart();
        return this.checkResult(result);
    }

    /**
     * 删除已保存的路径
     *
     * @param ip
     * @param path      要删除的路径名称
     * @param removeAll 是否删除所有路径, 可选参数, 默认为否
     * @return
     */
    public boolean removePath(String ip, String path, boolean removeAll) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.removePath(path, removeAll);
        return this.checkResult(result);
    }

    /**
     * 查询已保存的所有路径名称
     *
     * @return 名称列表, 若没有路径则返回空列表
     */
    public boolean queryPathLists(String ip) {
        XMateRobot robot = Optional.ofNullable(robotMap.get(ip))
                .orElseThrow(() -> new RuntimeException(String.format("[%s]设备未登录，请先登录！", ip)));
        RobotResult result = robot.queryPathLists();
        return this.checkResult(result);
    }

}